import numpy as np
# from matplotlib import pyplot as plt

# B样条曲线
class BSpline():

    def __init__(self,points,k=3):
        self.points = np.array(points)
        self.n = len(points) #节点个数
        self.k = k #曲线次数
        self.knot = np.zeros(self.k+self.n+1)
        self.pieces = self.n-self.k+1
        self.knot = np.linspace(0,1,self.pieces)
        self.knot = np.pad(self.knot,(self.k,self.k),'edge')
    
    #基函数递推方法: de Boor-Cox
    def base_func(self,t,i,k):
        if (k == 0):
            if (t>=self.knot[i] and t<self.knot[i+1]):
                return 1
            else:
                return 0
        else:
            if (t<self.knot[i] or t>self.knot[i+k+1]):
                return 0
            else:
                coef1 = 0
                coef2 = 0
                denominator = self.knot[i+k] - self.knot[i]
                if (denominator != 0):
                    coef1 = (t - self.knot[i])/denominator
                denominator = self.knot[i+k+1] - self.knot[i+1]
                if (denominator != 0):
                    coef2 = (self.knot[i+k+1] - t)/denominator
                value = coef1 * self.base_func(t,i,k-1) + coef2 * self.base_func(t,i+1,k-1)
                return value

    # nn: 采样点个数
    def curve(self,nn): 
        t_map = np.linspace(0,1,nn)
        curve = np.array([self.points[0]])
        for t in t_map:
            curve_point = np.array([0,0]).astype(np.float64)
            for i in range(self.n):
                Ft_ik = self.base_func(t,i,self.k)
                curve_point += self.points[i] * Ft_ik
            curve = np.append(curve,[curve_point],axis=0)
        # return curve[:-1] # 结尾有个[0,0]点，去掉
        return curve  # 保留[0,0]点，使得路径起点为[0,0]




# points = np.array([[5,1],[1,5],[3,7],[5,5],[5,5],[7,7],[9,5],[5,1]])
# px = points[:,:1].flatten()
# py = points[:,1:].flatten()
# bs = BSpline(points)
# curve = bs.curve(50)
# print(curve.shape)
# cx = curve[:,:1].flatten()
# cy = curve[:,1:].flatten()

# plt.title('Quasi-uniform B-Spline Curve')
# plt.scatter(px,py,s=5,c='r')
# plt.scatter(cx,cy,s=1,c='b')
# plt.show()